#define FLOAT_DECIMAL_PLACES 2

void loop()
{
  
   //-------------------------------------------------------------------
  int samples = 8;
  int delayBetweenSamples = 50;
  boolean calibrate = (digitalRead(CALIBRATION_SWITCH_D_PIN) == HIGH);
  sensor.read(samples, delayBetweenSamples, calibrate);
  
 //--------------------------------------------------------------------

  String s1 = "U:" + String(sensor.getPercentU()) + " D:"  + String(sensor.getPercentD());
  String s2 = "L:" + String(sensor.getPercentL()) + " R:" +  String(sensor.getPercentR());
  lcd.clear();
  lcd.displayText(s1,s2);


  int ratio = sensor.getRatioThreshold();
  int delta = sensor.getDeltaThreshold();
  
  //Correciones verticales
  if (sensor.getUdRatio() > ratio && sensor.getUdDelta() > delta)
  {
    moveUp(); //mucha luz en LDR UP y poca en LDR DOWN --> hay que subir para equilibrar
    DEBUG_PRINT(F("Move UP\n"));
    lcd.showCustomChar(ARROW_U, 0, 0);
  }
  else if (sensor.getDuRatio() > ratio && sensor.getUdDelta() < -1*delta)
  {
    moveDown();
    DEBUG_PRINT(F("Move DOWN\n"));
    lcd.showCustomChar(ARROW_D, 0, 0);
  }
  else
  {
    stopVertical();
    DEBUG_PRINT(F("STOP VERTICAL\n"));
  }

  //Correciones horizontales
  if (sensor.getLrRatio() > ratio && sensor.getLrDelta() > delta)
  {
    moveLeft();
    DEBUG_PRINT(F("Move LEFT\n"));
    lcd.showCustomChar(ARROW_L, 1, 0);
  }
  else if (sensor.getRlRatio() > ratio && sensor.getLrDelta() < -1*delta)
  {
    moveRight();
    DEBUG_PRINT(F("Move RIGHT\n"));
    lcd.showCustomChar(ARROW_R, 1, 0);
  }
  else
  {
    stopHorizontal();  
    DEBUG_PRINT(F("STOP HORIZONTAL\n"));
  }
  
  
  displayBar(sensor.getU(), 'U');
  displayBar(sensor.getD(), 'D');
  displayBar(sensor.getL(), 'L');
  displayBar(sensor.getR(), 'R');
  
  
  // Reseteo del contador del watchdog timer para asegurarnos de que no se resetea el sistema si todo 
  // funciona como es debido. Si no se llama a esta función acabará por agotarse el tiempo de espera y
  //se reiniciará el arduino empezando de nuevo
  wdt_reset(); 
  //Un pequeño delay para no volver loco al puerto serie
  delay(LOOP_DELAY);
  
}

void displayBar(int number, char c)
{
  int maxSteps = 60;
  int steps = maxSteps * (float)((float)number / 1024.0)  ;
  
  for (int i = 0; i < steps; i++)
    Serial.print(c);
  Serial.print(" ");
  Serial.print(number);
  Serial.println();
}

